/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-08-05 00:40:15
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-10 02:22:26
 * @FilePath: /esp32_motor/motor/algorithm /smbObserver.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "motor_priv.h"
#include "smbObserver.h"
#include <math.h>

SmbObserverObj smbObserver;
AngleSMO       smbAngle;

// /*电流预测*/
// void SMO_currCale(void)
// {
//     SinCos sinCos;

//     double IalphaError_Limit = 0, IbetaError_Limit = 0;
//     /*	Sliding mode current observer	*/
//     smbAngle.EstIalpha = smbAngle.EstIalpha * smbAngle.Fsmopos +
//                          (smbAngle.Valpha - smbAngle.Ealpha - smbAngle.Zalpha) * smbAngle.Gsmopos;
//     smbAngle.EstIbeta =
//         smbAngle.EstIbeta * smbAngle.Fsmopos + (smbAngle.Vbeta - smbAngle.Ebeta - smbAngle.Zbeta) * smbAngle.Gsmopos;

//     /*	Current errors	*/
//     smbAngle.IalphaError = smbAngle.EstIalpha - smbAngle.Ialpha;
//     smbAngle.IbetaError  = smbAngle.EstIbeta - smbAngle.Ibeta;

//     /*  Sliding control calculator	*/
//     /* smbAngle.Zalpha=smbAngle.IalphaError*smbAngle.Kslide/smbAngle.E0) where E0=0.5 here*/
//     //限制赋值函数
//     IalphaError_Limit = CONSTRAIN(smbAngle.IalphaError, -smbAngle.E0, smbAngle.E0);
//     IbetaError_Limit  = CONSTRAIN(smbAngle.IbetaError, -smbAngle.E0, smbAngle.E0);

//     smbAngle.Zalpha   = IalphaError_Limit * smbAngle.Kslide;
//     smbAngle.Zbeta    = IbetaError_Limit * smbAngle.Kslide;

//     /*	Sliding control filter -> back EMF calculator	*/
//     smbAngle.Ealpha = smbAngle.Ealpha + (smbAngle.Zalpha - smbAngle.Ealpha) * smbAngle.Kslf;
//     smbAngle.Ebeta  = smbAngle.Ebeta + (smbAngle.Zbeta - smbAngle.Ebeta) * smbAngle.Kslf;
//     smbAngle.qAngle = FastGetAngelFromPhase(smbAngle.Ealpha, smbAngle.Ebeta);

//     /*pll锁相环*/
//     FastSinCos(smbAngle.qPllAngle, &sinCos);
//     smbAngle.pllErr    = smbAngle.Ebeta * sinCos.cosValue - smbAngle.Ealpha * sinCos.sinValue;
//     smbAngle.pllInterg += smbAngle.pllErr * smbAngle.pllKi;
//     smbAngle.pllUi     = smbAngle.pllErr * smbAngle.pllKp + smbAngle.pllInterg;
//     smbAngle.qPllAngle = smbAngle.qPllAngle + (int16_t)(smbAngle.pllUi * _Q2PI / _2PI);
//     smbAngle.qPllAngle = ElectricalQAngle(smbAngle.qPllAngle);
// }

// /*初始化*/
// void SMO_observerInit(void)// 电机参数初始化
// {
//     smbObserver.Rs      = MOTOR_RS/10;         // 电机电阻欧姆
//     smbObserver.Ls      = MOTOR_LS;         // 电机电感H
//     smbObserver.Ib      = 7.4;              // 电机的额定相电流
//     smbObserver.Vb      = (11.1 * _1_SQRT3);// 电机额定相电压  24*0.57735
//     smbObserver.Ts      = 0.0002;           // 代码运行周期，PWM中断周期 15KHZ
//     smbObserver.Fsmopos = exp((-smbObserver.Rs / smbObserver.Ls) * (smbObserver.Ts));
//     smbObserver.Gsmopos = 10 / ((smbObserver.Vb / smbObserver.Ib) * (1 / smbObserver.Rs) * (1 - smbObserver.Fsmopos));
//     printf("f=%f,g=%f\n", smbObserver.Fsmopos, smbObserver.Gsmopos);
//     // smbObserver.Fsmopos = 1 - smbObserver.Ts * (smbObserver.Rs / smbObserver.Ls);
//     // smbObserver.Gsmopos = smbObserver.Ts / MOTOR_LS;
//     // 计算滑膜 观测器系数， 根据实际电机调节滤波系数  Kslide和 Kslf
//     smbAngle.Fsmopos   = smbObserver.Fsmopos;
//     smbAngle.Gsmopos   = smbObserver.Gsmopos;
//     smbAngle.Kslide    = 0.58;//
//     smbAngle.Kslf      = 0.02;//
//     smbAngle.E0        = 0.3; //  标幺值1的一半
//     smbAngle.pllKp     = 15.f;
//     smbAngle.pllKi     = 0.5f;
//     smbAngle.pllErr    = 0.0f;
//     smbAngle.pllInterg = 0.0f;
//     smbAngle.pllUi     = 0.0f;
//     // Speed_estPare.speed_coeff = (float)(500 * 60 / (SMO_MotorPare.POLES * 4096.0f));//  1.831054463982609
// }